#region Include
using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
#endregion

namespace SakerSomFaller
{
    #region deklaration
    public class Domino : Microsoft.Xna.Framework.Game
    {
        //Config ===========
        Config config;
        Vector3 pushPoint;
        Vector3 forcePush;
        Vector3 cameraPosition;
        Vector3 lookAt;
        Vector3 upDirection;
        Vector3 positionOne;
        Vector3 positionTwo;
        //======================

        public const int NR_BRICKS = 4; 
        GraphicsDeviceManager graphics;
        SpriteBatch spriteBatch;
        GraphicsDevice device;

        Effect effect;
        Physics physicsEngine;

        Matrix viewMatrix;
        Matrix projectionMatrix;
        Polygon[] ground;
        private RigidBody[] rigidBodyList;
        private Couple[] coupleList;
        int nrCouples;
        public Domino()
        {
            graphics = new GraphicsDeviceManager(this);
            Content.RootDirectory = "Content";
        }

        private void SetUpCamera()
        {
            viewMatrix = Matrix.CreateLookAt(cameraPosition, lookAt, upDirection);

            projectionMatrix = Matrix.CreatePerspectiveFieldOfView(MathHelper.PiOver4, device.Viewport.AspectRatio, 1.0f, 300.0f);
        }

        protected override void Initialize()
        {
            graphics.PreferredBackBufferWidth = 600;
            graphics.PreferredBackBufferHeight = 600;
            graphics.IsFullScreen = false;
            graphics.ApplyChanges();
            Window.Title = "Saker Som Faller - Domino";

            base.Initialize();
        }
    #endregion
        #region createGround
        private void createGround(ref RigidBody rigBody)
        {
            rigBody.numberOfPolygons = 12;
            rigBody.polygonArray = new Polygon[rigBody.numberOfPolygons];

            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                rigBody.polygonArray[i].vertices = new VertexPositionColor[3];
            }
            Vector3[] vertices = new Vector3[8];
            vertices[0] = new Vector3(-10f, 0, 5f);
            vertices[1] = new Vector3(-10f, 0, -5f);
            vertices[2] = new Vector3(10f, 0, -5f);
            vertices[3] = new Vector3(10f, 0, 5f);
            vertices[4] = new Vector3(-10f, 1f, 5f);
            vertices[5] = new Vector3(10f, 1f ,5f);
            vertices[6] = new Vector3(10f, 1f ,-5f);
            vertices[7] = new Vector3(-10f, 1f ,-5f);

            rigBody.polygonArray[0].vertices[0].Position = vertices[0];
            rigBody.polygonArray[0].vertices[1].Position = vertices[1];
            rigBody.polygonArray[0].vertices[2].Position = vertices[2];


            rigBody.polygonArray[1].vertices[0].Position = vertices[2];
            rigBody.polygonArray[1].vertices[1].Position = vertices[3];
            rigBody.polygonArray[1].vertices[2].Position = vertices[0];

            rigBody.polygonArray[2].vertices[0].Position = vertices[4];
            rigBody.polygonArray[2].vertices[1].Position = vertices[5];
            rigBody.polygonArray[2].vertices[2].Position = vertices[6];

            rigBody.polygonArray[3].vertices[0].Position = vertices[6];
            rigBody.polygonArray[3].vertices[1].Position = vertices[7];
            rigBody.polygonArray[3].vertices[2].Position = vertices[4];


            rigBody.polygonArray[4].vertices[0].Position = vertices[0];
            rigBody.polygonArray[4].vertices[1].Position = vertices[3];
            rigBody.polygonArray[4].vertices[2].Position = vertices[5];

            rigBody.polygonArray[5].vertices[0].Position = vertices[5];
            rigBody.polygonArray[5].vertices[1].Position = vertices[4];
            rigBody.polygonArray[5].vertices[2].Position = vertices[0];

            rigBody.polygonArray[6].vertices[0].Position = vertices[3];
            rigBody.polygonArray[6].vertices[1].Position = vertices[2];
            rigBody.polygonArray[6].vertices[2].Position = vertices[6];

            rigBody.polygonArray[7].vertices[0].Position = vertices[6];
            rigBody.polygonArray[7].vertices[1].Position = vertices[5];
            rigBody.polygonArray[7].vertices[2].Position = vertices[3];

            rigBody.polygonArray[8].vertices[0].Position = vertices[2];
            rigBody.polygonArray[8].vertices[1].Position = vertices[1];
            rigBody.polygonArray[8].vertices[2].Position = vertices[7];

            rigBody.polygonArray[9].vertices[0].Position = vertices[7];
            rigBody.polygonArray[9].vertices[1].Position = vertices[6];
            rigBody.polygonArray[9].vertices[2].Position = vertices[2];

            rigBody.polygonArray[10].vertices[0].Position = vertices[1];
            rigBody.polygonArray[10].vertices[1].Position = vertices[0];
            rigBody.polygonArray[10].vertices[2].Position = vertices[4];

            rigBody.polygonArray[11].vertices[0].Position = vertices[4];
            rigBody.polygonArray[11].vertices[1].Position = vertices[7];
            rigBody.polygonArray[11].vertices[2].Position = vertices[1];
        }
        private void createMesh(ref RigidBody rigBody)
        {
            rigBody.numberOfPolygons = 12;
            rigBody.polygonArray = new Polygon[rigBody.numberOfPolygons];

            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                rigBody.polygonArray[i].vertices = new VertexPositionColor[3];
            }
            Vector3[] vertices = new Vector3[8];
            vertices[0] = new Vector3(-0.25f, 0, 1);
            vertices[1] = new Vector3(-0.25f, 0, -1);
            vertices[2] = new Vector3(0.25f, 0, -1);
            vertices[3] = new Vector3(0.25f, 0, 1);
            vertices[4] = new Vector3(-0.25f, 4, 1);
            vertices[5] = new Vector3(0.25f, 4, 1);
            vertices[6] = new Vector3(0.25f, 4, -1);
            vertices[7] = new Vector3(-0.25f, 4, -1);

            rigBody.polygonArray[0].vertices[0].Position = vertices[0];
            rigBody.polygonArray[0].vertices[1].Position = vertices[1];
            rigBody.polygonArray[0].vertices[2].Position = vertices[2];


            rigBody.polygonArray[1].vertices[0].Position = vertices[2];
            rigBody.polygonArray[1].vertices[1].Position = vertices[3];
            rigBody.polygonArray[1].vertices[2].Position = vertices[0];

            rigBody.polygonArray[2].vertices[0].Position = vertices[4];
            rigBody.polygonArray[2].vertices[1].Position = vertices[5];
            rigBody.polygonArray[2].vertices[2].Position = vertices[6];

            rigBody.polygonArray[3].vertices[0].Position = vertices[6];
            rigBody.polygonArray[3].vertices[1].Position = vertices[7];
            rigBody.polygonArray[3].vertices[2].Position = vertices[4];


            rigBody.polygonArray[4].vertices[0].Position = vertices[0];
            rigBody.polygonArray[4].vertices[1].Position = vertices[3];
            rigBody.polygonArray[4].vertices[2].Position = vertices[5];

            rigBody.polygonArray[5].vertices[0].Position = vertices[5];
            rigBody.polygonArray[5].vertices[1].Position = vertices[4];
            rigBody.polygonArray[5].vertices[2].Position = vertices[0];

            rigBody.polygonArray[6].vertices[0].Position = vertices[3];
            rigBody.polygonArray[6].vertices[1].Position = vertices[2];
            rigBody.polygonArray[6].vertices[2].Position = vertices[6];

            rigBody.polygonArray[7].vertices[0].Position = vertices[6];
            rigBody.polygonArray[7].vertices[1].Position = vertices[5];
            rigBody.polygonArray[7].vertices[2].Position = vertices[3];

            rigBody.polygonArray[8].vertices[0].Position = vertices[2];
            rigBody.polygonArray[8].vertices[1].Position = vertices[1];
            rigBody.polygonArray[8].vertices[2].Position = vertices[7];

            rigBody.polygonArray[9].vertices[0].Position = vertices[7];
            rigBody.polygonArray[9].vertices[1].Position = vertices[6];
            rigBody.polygonArray[9].vertices[2].Position = vertices[2];

            rigBody.polygonArray[10].vertices[0].Position = vertices[1];
            rigBody.polygonArray[10].vertices[1].Position = vertices[0];
            rigBody.polygonArray[10].vertices[2].Position = vertices[4];

            rigBody.polygonArray[11].vertices[0].Position = vertices[4];
            rigBody.polygonArray[11].vertices[1].Position = vertices[7];
            rigBody.polygonArray[11].vertices[2].Position = vertices[1];
        }
        #endregion
        #region LoadContent
        protected override void LoadContent()
        {

            physicsEngine = new Physics();

            config = new Config();
            pushPoint = config.pushPoint;
            forcePush = config.forcePush;
            cameraPosition = config.cameraPosition;
            lookAt = config.lookAt;
            upDirection = config.upDirection;
            positionOne = config.positionOne;
            positionTwo = config.positionTwo;

            spriteBatch = new SpriteBatch(GraphicsDevice);
            device = graphics.GraphicsDevice;
            effect = Content.Load<Effect>("effects");

            SetUpCamera();

            rigidBodyList = new RigidBody[NR_BRICKS+1];
            coupleList = new Couple[2*NR_BRICKS-1];
          

            for (int i = 0; i < NR_BRICKS-1; i++)
            {
                rigidBodyList[i] = new RigidBody();
                createMesh(ref rigidBodyList[i]);
                //Bricka1
                rigidBodyList[i].position = new Vector3(2*i,-0.0f,0);
                rigidBodyList[i].height = 4.0f;
                rigidBodyList[i].width = 2.0f;
                rigidBodyList[i].depth = 0.25f;

                coupleList[i].rigBodyA = i;
                coupleList[i].rigBodyB = i+1; 
            }


            //Specialfall - Sista biten
            rigidBodyList[NR_BRICKS-1] = new RigidBody();
            createMesh(ref rigidBodyList[NR_BRICKS-1]);
            rigidBodyList[NR_BRICKS-1].position = new Vector3(2*NR_BRICKS, 0f, 0);
            rigidBodyList[NR_BRICKS-1].height = 4.0f;
            rigidBodyList[NR_BRICKS-1].width = 2.0f;
            rigidBodyList[NR_BRICKS-1].depth = 0.25f;


            //Marken========================
            rigidBodyList[NR_BRICKS] = new RigidBody();
            createGround(ref rigidBodyList[NR_BRICKS]);
            rigidBodyList[NR_BRICKS].position = new Vector3(0, -3, 0);
            rigidBodyList[NR_BRICKS].height = 100.0f;
            rigidBodyList[NR_BRICKS].width = 100.0f;
            rigidBodyList[NR_BRICKS].depth = 0.2f;
            rigidBodyList[NR_BRICKS].rotation = new Matrix3(0, 1, 0, -1, 0, 0, 0, 0, 1);
            rigidBodyList[NR_BRICKS].stationary = true;


            int coupleIndex = NR_BRICKS - 1;
            for (int i = 0; i < NR_BRICKS; i++)
            {
                coupleList[coupleIndex].rigBodyA = NR_BRICKS;
                coupleList[coupleIndex].rigBodyB = i;
                coupleIndex++;
            }
            nrCouples = coupleIndex;

            physicsEngine.rigidBodyList = rigidBodyList;
            physicsEngine.coupleList = coupleList;

            
            physicsEngine.initPhysics(ref rigidBodyList[0], forcePush, pushPoint);
            //Initphysics  =================================
            for (int i = 1; i <= NR_BRICKS; i++)
            {
                physicsEngine.initPhysics(ref rigidBodyList[i], new Vector3(0f, 0, 0), new Vector3(0f));
            }
            //==========================================================
            for (int i = 0; i < nrCouples; i++)
            {
                physicsEngine.calculateSeparatingPlane(ref coupleList[i]);
            }

        }
        #endregion
        #region unLoadContent
        protected override void UnloadContent()
        {
        }

        #endregion

        protected override void Update(GameTime gameTime)
        {
            if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed) //|| gameTime.TotalGameTime > new TimeSpan(3000000))
                this.Exit();


            for (int i = 0; i < nrCouples; i++)
            {
                if (!physicsEngine.calculateSeparatingPlane(ref coupleList[i]))
                {
                    Console.WriteLine("Kollision!");

                    float h = physicsEngine.h;
                    /*while (!physicsEngine.calculateSeparatingPlane(ref coupleList[i]))
                    {
                        h = h / 2;
                        physicsEngine.stepBack(ref rigidBodyList[coupleList[i].rigBodyA]);
                        physicsEngine.stepBack(ref rigidBodyList[coupleList[i].rigBodyB]);
                        rigidBodyList = physicsEngine.rigidBodyList;

                        physicsEngine.updatePhysics(ref rigidBodyList[coupleList[i].rigBodyA], h);
                        physicsEngine.updatePhysics(ref rigidBodyList[coupleList[i].rigBodyB],h);
                        rigidBodyList = physicsEngine.rigidBodyList;

                    }*/

                        physicsEngine.findContactPoints(ref coupleList[i]);
                        //if(coupleList[i].rigBodyA == NR_BRICKS)
                           physicsEngine.contactList = physicsEngine.interpolateContacts(physicsEngine.contactList);
                        physicsEngine.resolveCollision(ref physicsEngine.contactList);
                        physicsEngine.contactList.Clear();
                        rigidBodyList = physicsEngine.rigidBodyList;

                }
            }



            for (int i = 0; i < rigidBodyList.Count() - 1; i++)
            {
                RigidBody[] rbl = rigidBodyList;
                physicsEngine.updatePhysics(ref rbl[i], 0.01f);
            }
            rigidBodyList = physicsEngine.rigidBodyList;



                /*float h = physicsEngine.h;

                while (!physicsEngine.calculateSeparatingPlane(ref couple))
                {
                    h = h / 2;
                    physicsEngine.stepBack(ref couple.rigBodyA);
                    physicsEngine.stepBack(ref couple.rigBodyB);

                    physicsEngine.updatePhysics(ref couple.rigBodyA, h);
                    physicsEngine.updatePhysics(ref couple.rigBodyB, h);

                }*/
                
            /*
                physicsEngine.findContactPoints(ref couple);
                physicsEngine.collidingContact(ref couple);
                */

            

            base.Update(gameTime);
        }
        #region drawRigidBody
        private void drawRigidBody(ref RigidBody rigBody, Boolean lsd)
        {
            
            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                VertexPositionColor[] world = new VertexPositionColor[3];

                world = physicsEngine.localToWorldVPC(ref rigBody, ref rigBody.polygonArray[i]);

                if (lsd)
                {
                    world[0].Color = new Color(i * 50, i, i);
                    world[1].Color = new Color(i, i * 50, i);
                    world[2].Color = new Color(i, i, i * 50);
                }

                //Console.WriteLine("World: " + world[0]);
                //Console.WriteLine("World: " + world[1]);
                //Console.WriteLine("World: " + world[2]);

                device.DrawUserPrimitives(PrimitiveType.TriangleList, world, 0, 1, VertexPositionColor.VertexDeclaration);
            }

        }
        #endregion
        #region Draw
        protected override void Draw(GameTime gameTime)
        {
            device.Clear(Color.LightGray);

            RasterizerState rs = new RasterizerState();
            rs.CullMode = CullMode.None;
            device.RasterizerState = rs;

            effect.CurrentTechnique = effect.Techniques["ColoredNoShading"];
            effect.Parameters["xView"].SetValue(viewMatrix);
            effect.Parameters["xProjection"].SetValue(projectionMatrix);
            effect.Parameters["xWorld"].SetValue(Matrix.Identity);
            foreach (EffectPass pass in effect.CurrentTechnique.Passes)
            {
                pass.Apply();

                for(int i = 0; i < NR_BRICKS; i++)
                {
                    RigidBody rigBody = rigidBodyList[i];
                    drawRigidBody(ref rigBody,true);
                }

                drawRigidBody(ref rigidBodyList[NR_BRICKS], false);

            }

            base.Draw(gameTime);
        }
        #endregion
    }
}